# 用于启动moveit+rviz，请不要单独使用，它是为了配合其他东西用的
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node

def generate_launch_description():
    
    moveit_config = get_package_share_directory('moveit_config')
    
    # virtual joint
    virtual_joint = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([moveit_config+"/launch/static_virtual_joint_tfs.launch.py"])
    )
    
    move_group = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([moveit_config+"/launch/move_group.launch.py"])
    )
    
    # joint_state_publisher
    joint_state_publisher = Node(
            package="joint_state_publisher",
            executable="joint_state_publisher",
            parameters=[
                { 'source_list' : ['/joint_state_broadcaster/joint_states']}
            ],
            output="screen",
    )
    
    # rviz
    rviz = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([moveit_config+"/launch/moveit_rviz.launch.py"])
    )
    
    
    return LaunchDescription([
        virtual_joint,
        joint_state_publisher,
        rviz,
        move_group,
    ])